#include "../Tools/GLHeaders.hpp" 
#include "../Tools/Bone.hpp"
#include "../Tools/Vector.hpp"
#include "../Tools/Chrono.hpp"
#include "Objet.hpp"
#include "Obj0.hpp"
#include "Obj1.hpp"
#include "Obj2.hpp"
#include "Obj3.hpp"
#include <cmath>

Objet::Objet(void){
	//Initialise Bone
	time = new Chrono;
	bone0 = new Bone(2);
	bone1 = new Bone(0);
	bone2 = new Bone(0);
	bone3 = new Bone(1);
	obj0 = new Obj0;
	obj1 = new Obj1;
	obj2 = new Obj2;
	obj3 = new Obj3;

	//Initialise Model for each bones
	bone0->SetModel(obj0);
	bone1->SetModel(obj1);
	bone2->SetModel(obj2);
	bone3->SetModel(obj3);
		
	//Initialise Connection between bones
	bone0->SetConnect(0, bone1);
	bone0->SetConnect(1, bone3);
	bone3->SetConnect(0, bone2);


	bone1->SetTranslation(0.0f, 1.0f, 0.0f);
	bone2->SetTranslation(4.0f, 1.0f, 0.0f);
	bone3->SetTranslation(4.0f, 0.0f, 0.0f);
	time->Reset();
  speed = 1.0f;
	activate = false;
}

Objet::~Objet(void){
	delete time;
	delete bone0;
	delete bone1;
	delete bone2;
	delete bone3;
	delete obj0;
	delete obj1;
	delete obj2;
	delete obj3;
}

void Objet::Draw(void){
	Vector tmp;
	Vector tmp2;
	GLfloat angle;
	GLfloat alpha;
	GLfloat dx;
	GLfloat dy;

	glPushMatrix();
	if(activate){
		obj0->Activate();
		obj1->Activate();
		obj2->Activate();
		obj3->Activate();
		time->Activate();
	}
	else{
		obj0->Desactivate();
		obj1->Desactivate();
		obj2->Desactivate();
		obj3->Desactivate();
		time->Desactivate();
	}

	angle = 45.0f + 45.0f*sinf(time->GetTime()*speed*M_PI-M_PI/2.0f);
	dx =  4.0f + 4.0f*cosf(angle*M_PI/180.0f) - 1.0f*sinf(angle*M_PI/180.0f);
	dy = -1.0f + 4.0f*sinf(angle*M_PI/180.0f) + 1.0f*cosf(angle*M_PI/180.0f);
	alpha = atan(dy/dx) / M_PI*180.0f;
	bone3->SetRotation(angle, 0.0f, 0.0f, 1.0f);
	bone1->SetRotation(alpha, 0.0f, 0.0f, 1.0f);
	bone2->SetRotation(-(angle-alpha), 0.0f, 0.0f, 1.0f);
	
	//bone0->SetTranslate(cosf(time->GetTime()*speed*M_PI), 0.0f, 0.0f);
	
	bone0->Draw();
	glPopMatrix();
}

void Objet::Desactivate(void){
	activate = false;
}

void Objet::Activate(void){
	activate = true;
}
